/**
 *
 *	@file FR_math.c - c implementation file for basic fixed 
 *                              radix math routines
 *	
 *	@copy Copyright (C) <2001-2014>  <M. A. Chatterjee>
 *  @author M A Chatterjee <deftio [at] deftio [dot] com>
 *	@version 1.01 M. A. Chatterjee, cleaned up naming
 *
 *  This file contains integer math settable fixed point radix math routines for
 *  use on systems in which floating point is not desired or unavailable.
 *
 *	This software is provided 'as-is', without any express or implied
 *	warranty. In no event will the authors be held liable for any damages
 *	arising from the use of this software.
 *
 *	Permission is granted to anyone to use this software for any purpose,
 *	including commercial applications, and to alter it and redistribute it
 *	freely, subject to the following restrictions:
 *
*	1. The origin of this software must not be misrepresented; you must not
 *	claim that you wrote the original software. If you use this software
 *	in a product, please place an acknowledgment in the product documentation.
 *
 *	2. Altered source versions must be plainly marked as such, and must not be
 *	misrepresented as being the original software.
 *
 *	3. This notice may not be removed or altered from any source
 *	distribution.
 *
 */
 

#include "FR_math.h"


/*=======================================================
 * FR_FixMuls (x*y signed NOT saturated mul)
 * User must keep track of new radix point after using this function
 */

s32 FR_FixMuls(s32 x, s32 y) /* signed not saturated */
{
	s32 z,sign = (x < 0)? (y>0) : (y < 0);

	x = FR_ABS(x);
	y = FR_ABS(y);
	z = FR_FIXMUL32u(x,y);
	
	return sign?-z:z;	
}

/*=======================================================
 * FR_FixMulSat (x*y saturated multiply)
 * User must keep track of new radix point after using this function
 */

s32 FR_FixMulSat(s32 x, s32 y)
{
	s32 z,h,l,m1,m2;
	int sign = ( ((x<0)&& (y>0)) || ((x>0)&&(y<0)) ) ?1:0;
	
	x = FR_ABS(x);
	y = FR_ABS(y);
	h  = ((x>>16)*(y>>16));	
	m1 = ((x>>16)*(y&0xffff));
	m2 = ((y>>16)*(x&0xffff));
	l  = (((x&0xffff)*(y&0xffff)));
	
	z = (h<<16)+m1+m2+l;

	if (h&0xffff8000)  /* TODO: make better saturation check -- this might be a bug */
		z = 0x7fffffff;
	else
	{
		z = (h<<16) + m1;
		if (z < 0)
			z = 0x7fffffff;
		else
		{
			z+= m2;
			if (z < 0)
				z = 0x7fffffff;
			else
			{
				z += l;
				if (z < 0)
					z = 0x7fffffff;
			}
		}
	}
	return sign?-z:z;	
}

/*=======================================================
  FR_FixAddSat (x+y saturated add)
  programmer must align radix points before using this function
 */
s32 FR_FixAddSat(s32 x, s32 y)
{
	s32 sum = x+y;
	if (x < 0)
	{
		if (y < 0)
			return (sum >= 0)?0x80000000:sum;
	}
	else
	{
		if (y >= 0)
			return (sum <= 0)?0x7fffffff:sum;	
	}
	return sum;
}


/* Cosine table in s0.15 format, 1 entry per degree
 * used in all trig functions (sin,cos,tan, atan2 etc)
 */
s16 const static gFR_COS_TAB_S0d15[]={
    32767,32762,32747,32722,32687,32642,32587,32522,32448,32363,
    32269,32164,32050,31927,31793,31650,31497,31335,31163,30981,
    30790,30590,30381,30162,29934,29696,29450,29195,28931,28658,
    28377,28086,27787,27480,27165,26841,26509,26168,25820,25464,
    25100,24729,24350,23964,23570,23169,22761,22347,21925,21497,
    21062,20620,20173,19719,19259,18794,18323,17846,17363,16876,
    16383,15885,15383,14875,14364,13847,13327,12803,12274,11742,
    11207,10668,10125, 9580, 9032, 8481, 7927, 7371, 6813, 6252,
     5690, 5126, 4560, 3993, 3425, 2856, 2286, 1715, 1144,  572,
        0};
    
/* cosine with integer input precision in degrees, returns s0.15 result
 */

s16 FR_CosI(s16 deg) 
{
	deg = deg%360; /* this is an expensive operation*/
	if (deg > 180) { deg -= 360; }
	else if (deg < -180) { deg += 360;}
	
	if (deg >= 0)
		return   deg <=  (90) ?  gFR_COS_TAB_S0d15[ deg ]:-gFR_COS_TAB_S0d15[180-deg];		
	else
		return   deg >= (-90) ?  gFR_COS_TAB_S0d15[-deg ]:-gFR_COS_TAB_S0d15[180+deg];	
}
/* sin with integer input precision in degrees, returns s0.15 result
 */
s16 FR_SinI(s16 deg)
{
	return FR_CosI(deg-90);
}

/* cos() with fixed radix precision, returns interpolated s0.15 result
 */
s16 FR_Cos (s16 deg,u16 radix)
{
	s16 i,j;
	i = FR_CosI(deg>>radix);
	j=  FR_CosI((deg>>radix)+1);
	return i+(((j-i)*(deg&((1<<radix)-1)))>>radix);
}

/* sin() with fixed radix precision,  returns interpolated s0.15 result
 * could be a macro..
 */
s16 FR_Sin (s16 deg,u16 radix)
{
	return FR_Cos(deg-(90<<radix),radix);
}

s16 const static gFR_TAND_TAB[]={
        1,     572,    1144,    1717,    2291,    2867,    3444,    4023,    4605,    5189,
     5777,    6369,    6964,    7564,    8169,    8779,    9395,   10017,   10646,   11282,
    11926,   12578,   13238,   13908,   14588,   15279,   15981,   16695,   17422,   18163,
    18918,   19688,   20475,   21279,   22101,   22943,   23806,   24691,   25600,   26534,
	27494,   28483,   29503,   30555,   31642,   32767};

/* tan with s15.16 result 
 * tan without table.
 * note: tan(90)  returns   32767 
 * and   tan(270) returns (-32768) (e.g. no div by zero)
 */
/*
s32 FR_TanI (s16 deg) 
{
	s32 c = FR_CosI(deg);
	s32 s = FR_SinI(deg);
	return (c!=0)?(s<<FR_TRIG_PREC)/c : (s>=0)?(FR_TRIG_MAXVAL<<FR_TRIG_PREC):(FR_TRIG_MINVAL<<FR_TRIG_PREC);
}
*/
#define FR_TN(a) (((a)<= 45)?gFR_TAND_TAB[(a)]:(FR_TRIG_MAXVAL<<FR_TRIG_PREC)/(gFR_TAND_TAB[90-(a)]))	
s32 FR_TanI (s16 deg)
{
	deg = deg%360; /* this is an expensive operation */
	if (deg > 180) { deg -= 360; }
	else if (deg < -180) { deg += 360;}

	if (90  == deg)
		return (FR_TRIG_MAXVAL<<FR_TRIG_PREC);
	if (270 == deg)
		return -(FR_TRIG_MAXVAL<<FR_TRIG_PREC);

	if (deg >= 0)
		return   deg <=  (90) ?  FR_TN( deg ):-FR_TN(180-deg);		
	else
		return   deg >= (-90) ? -FR_TN(-deg ): FR_TN(180+deg);

}	
/* Tan with s15.16 result with fixed radix input precision, returns interpolated s15.16 result
 */
 
s32 FR_Tan (s16 deg, u16 radix)
{
	s16 i,j;
	i = FR_TanI(deg>>radix);
	j=  FR_TanI((deg>>radix)+1);
	return FR_INTERPI(i,j,deg,radix);/*i+(((j-i)*(deg&((1<<radix)-1)))>>radix);*/
}	

/* Inverse Trig
 * Ugly looking acos with bin search (working):
 */
s16 FR_acos (s32 input, u16 radix)
{
	s16 r=45, s=input, x=46,y,z;	
	
	input = FR_CHRDX(input,radix,FR_TRIG_PREC); /* chg radix to s0.15 */
	
	// +or- 1.0000 is special case as it doesn't fit in table search 
	if ((input&0xffff)==0x8000) //? shouldn't it be: (input&7fff)!=0 
		return (input<0) ? 180: 0;
	input = (FR_ABS(input)) & ((1<<radix)-1);	
	while (x>>=1)
		r+= (input < gFR_COS_TAB_S0d15[r]) ? x : -x;
	
	r+= (input <  gFR_COS_TAB_S0d15[r]) ? 1 : -1;
	r+= (input <  gFR_COS_TAB_S0d15[r]) ? 1 : -1;
	
	x = FR_ABS(input - gFR_COS_TAB_S0d15[r]);
	y = FR_ABS(input - gFR_COS_TAB_S0d15[r+1]);
	z = FR_ABS(input - gFR_COS_TAB_S0d15[r-1]);
	r = (x<y) ? r: r+1;
	r = (x<z) ? r: r-1;

	return (s>0) ? r:180-r;;	 
}


s16 FR_asin (s32 input, u16 radix)
{
	return 90-FR_acos(input,radix);	 
}
s16 FR_atan2 (s32 y, s32 x, u16 radix)
{
	int q;
	if (0==x)
		return y < 0 ? FR_TRIG_MINVAL : FR_TRIG_MAXVAL;
	
	q = (y >= 0) ? ( (x>= 0) ? 0 : 1) : ((x>=0) ? 3 : 2);
	return q;
}

/* Raise x to the xth power
 */                  

#define FR_kPOW2COEF_PREC	(4)
u16 static const gPOW2_ICOEF[17] = { 
	    0, 2902, 5932, 9096,12400,15850,19454,23216,27146,
	31249,35534,40009,44682,49562,54658,59979,65535}; /* 16bit prec */

/* Accuracy vs space tradeoff:
 * can save some space by comment out above coef and using smaller table here.
 *
#define FR_kPOW2COEF_PREC	(3)
u16 static const gPOW2_ICOEF[9] = {0,5932,12400,19454,27146,35534,44682,54658,65535}; // 16bit prec
 */

//TODO: keep prec up... should be input# of bits + 14 (includes interp error etc..)
// pow2: 383 bytes of 32bit x86 code under gcc cygwin with tables counted (and before output radix stuff added)
//e.g. outputradix = input+14 bits of prec so 
// if input > 30 output = MAXINTu32
// if input+14 < 20 output radix = 30-input
// if input+14 < 16 ouptut radix = 16 (e.g. don't just slam the radix point way to the left we don't have that precision anyway.
s32 FR_pow2(s32 input, u16 radix)
{
	s32 flr,frac,k,j,sc;
	flr  = FR_INT(input,radix);
	frac = FR_FRACS(input,radix,FR_kPOW2COEF_PREC);
	k = (s32)(gPOW2_ICOEF[frac]);
	j = (s32)(gPOW2_ICOEF[frac+1]);
	sc = ((j-k)>>8)+((j-k)>>10)+((j-k)>>11); /* slope correction */
	frac = ((FR_FRACS(input,radix,15))-(FR_FRACS(frac,FR_kPOW2COEF_PREC,15)))<<FR_kPOW2COEF_PREC;
	if (frac <= (1<<(15-1)))
		sc = FR_INTERP(0,sc,frac,14);
	else
		sc = FR_INTERP(sc,0,frac-(1<<(15-1)),14);

	k	= FR_INTERP(k,j,frac,15)-sc;

	if (flr >=0) /* positive powers of 2 */
	{
		k= FR_CHRDX((k<<flr),16,radix);
		j= (1<<flr)<<radix;
	}
	else /* negative powers of 2 */
	{
		flr = -flr;
		k= FR_CHRDX((k>>flr),16,radix);
		j= (radix-flr)>=0 ? 1<<(radix-flr):0;	
	}
	return j+k;
}
/*
s32 FR_exp(s32 input, u16 radix)
{
	return FR_pow2(FR_SLOG2E(input),radix);
}

s32 FR_pow10(s32 input, u16 radix)
{
	if (FR_FRAC(input,radix))
		return FR_pow2(FR_SLOG2_10(input),radix);
	else 
	{	
		input = FR_INT(input,radix);
		if (input >=0)
		{
			s32 x=10;	
			while (input--)
				x = FR_SMUL10(x); 
			return x<<radix;
		}
		return FR_pow2(FR_SLOG2_10(input),radix);
	}
}
*/

/*log2 returns FR_LOG2MIN if input <= 0 */
s32 FR_log2(s32 input, u16 radix, u16 output_radix)
{
	s32 h=16;//,frac = FR_FRAC(input,radix);
	while (h>>=1) {
		if (input>(1<<h))
			{input>>=h;} // i+=h; input>>=h
	}

	return input <=0 ? FR_LOG2MIN : FR_CHRDX(input,radix,output_radix);
}

s32 FR_ln(s32 input, u16 radix, u16 output_radix)
{
	s32 r = FR_log2(input,radix,output_radix);
	return FR_SrLOG2E(r); /* Note: return FR_SrLOG2E(FR_log2()) would be a very ugly macro expansion! */
}

s32 FR_log10(s32 input, u16 radix, u16 output_radix)
{
	s32 r = FR_log2(input,radix,output_radix);
	return FR_SrLOG2_10(r);/* Note: return FR_SrLOG2_10(FR_log2()) would be a very ugly macro expansion! */
}

/***************************************
 FR_printNum write out fixed radix number with space padding
  equiavlent ot %f in printf family
  myNum = 12.34 // in fixed num
  e.g. printf("%4.2f",myNum ) ==> "  12"  
   

    printf("test fr math rad \n");

    FR_printNumF (putchar,  123456   , 0, 3, 0);    printf("\n");
    FR_printNumF (putchar,  123456<<13 , 13, 3, 4);    printf(":\n");
    FR_printNumF (putchar,  D2FR(1234.5678,13)   , 13, 3, 6);    printf(":\n");
    FR_printNumF (putchar,  D2FR(-1234.5678,13)   , 13, 3, 6);    printf(":\n");
 */

int FR_printNumF(int(*f)(char), s32 n, int radix, int pad, int prec) 
{
    int t=10,s=0,r=FR_FRAC(n,radix);
    n=FR_INT(n,radix); 
    if (f) {
        if(n < 0) { n = -n, s=1;pad--;}
        while ( (n/t) > 0) { t*=10; pad--;}
        while (pad-- > 0) {f(' ');}
        if (s) f('-');
        while ( t>=10) {  
            t/=10;
            f((char)((n/t)%10)+'0');          
        }        
        if(prec || r) 
        {
            f('.');
            s = 1<<(radix);
            if (r) {
                while (s)  {
                    r*=10;
                    s/=10;
                } 
            }
            r>>=radix;
            //r++;
            t=1;
            while ( (r/t) > 0) { t*=10;}
            while ((t>=10)&&(prec)) {  
                t/=10; prec--;
                f((char)((r/t)%10)+'0');  
            }
            while (prec-- > 0) {f('0');}
        }
        return 0;
    }
    return -1;
}
/***************************************
 FR_printNumD write out a decimal integer with space padding
  equiavlent ot %d in printf family
  int num = 12
  e.g. printf("%4d",num) ==> "  12"  
   
 */
int FR_printNumD( int (*f)(char), int n, int pad )
{
    int t=10,s=0;
    if (f) {
        if(n < 0) { n = -n, s=1;pad--;}
        while ( (n/t) > 0) { t*=10; pad--;}
        while (pad-- > 0) {f(' ');}
        if (s) f('-');
        while ( t>=10) {  
            t/=10;
            f((char)((n/t)%10)+'0');          
        }        
        return FR_S_OK;
    }
    return FR_E_FAIL;
}
/***************************************
 FR_printNumH write out a integer as hex
 if (showPrefix == true ) 
    print "0x"
 else
    //no prefix printed
 */

int FR_printNumH( int (*f)(char), int n, int showPrefix ){
    int d,x = ((sizeof(int))<<1)-1;
    if (f) {
        if (showPrefix) {
            f('0');
            f('x');
        }

        do  {
            d = (n >> (x<<2))&0xf;
            d = (d > 9) ? (d-0xa + 'a') : (d + '0');
            f(d);
        }while (x--);
        return FR_S_OK;
    }
    return FR_E_FAIL;
}
